package nezumi.state;

import java.awt.Point;
import java.awt.geom.Point2D;
import java.util.LinkedList;
import java.util.logging.Logger;

import nezumi.Constants;
import nezumi.util.AngleGeometry;

public class AgentState {

	private static final double compassStdDev = Math
			.toRadians(Constants.COMPASS_STD_DEV);

	protected static final Logger logger = Logger.getLogger(AgentState.class
			.getName());

	// orientation is an angle in radians
	// measures deviation from the virtual north (x axis)
	final private LinkedList<Double> orientationHistory;
	final private LinkedList<Point2D> positionHistory;
	final private Point2D startPos;
	final private State state;
	private double lPowOut = 0, rPowOut = 0;

	public AgentState(State state) {
		super();
		this.state = state;
		this.positionHistory = new LinkedList<Point2D>();
		this.startPos = new Point2D.Double(0.0, 0.0);
		this.positionHistory.add(startPos);
		this.orientationHistory = new LinkedList<Double>();
		this.orientationHistory.add(Math.PI);
	}

	public AgentState(State state, Point2D startPos, double orientation) {
		super();
		this.state = state;
		this.positionHistory = new LinkedList<Point2D>();
		this.startPos = startPos;
		this.positionHistory.add(startPos);
		this.orientationHistory = new LinkedList<Double>();
		this.orientationHistory.add(orientation);
	}

	public Point2D getCurrPos() {
		return this.positionHistory.getLast();
	}

	public Point getCurrPosInt() {
		Point2D currPos = this.positionHistory.getLast();
		return new Point((int) Math.round(currPos.getX()), (int) Math
				.round(currPos.getY()));
	}

	/**
	 * Update both the agent's position and orientation according to the last
	 * motor command.
	 * 
	 * @param lPowIn
	 * @param rPowIn
	 */
	public void updateAgentState(double lPowIn, double rPowIn) {

		final double lOutPow = (lPowIn + this.lPowOut) / 2;
		final double rOutPow = (rPowIn + this.rPowOut) / 2;

		// linear movement
		final double lin = (lOutPow + rOutPow) / 2;
		// rotation
		final double rot = (rOutPow - lOutPow) / (Constants.MOUSE_RADIUS * 2);

		// update orientation
		final double orientation = AngleGeometry
				.normalizeAngle(orientationHistory.getLast() + rot);

		final Point2D currPos = this.positionHistory.getLast();

		final double newx = currPos.getX() + Math.cos(orientation)
				* (lin / Constants.MAP_GRANULARITY);
		final double newy = currPos.getY() + Math.sin(orientation)
				* (lin / Constants.MAP_GRANULARITY);
		Point2D newPos = new Point2D.Double(newx, newy);

		// add to history
		this.positionHistory.add(newPos);
		this.orientationHistory.add(orientation);

		// update the visited grid
		final MapState mstate = this.state.getMapState();
		mstate.updateVisitedGrid(newPos);

		// update the occupancy grid
		mstate.clearOccupancyGridAroundPos(newPos);

		// don't let history grow too big
		if (this.orientationHistory.size() > Constants.COMPASS_DELAY + 1)
			this.orientationHistory.poll();
		if (this.positionHistory.size() > Constants.BEACON_DELAY + 1)
			this.positionHistory.poll();

		// update power
		this.lPowOut = lOutPow;
		this.rPowOut = rOutPow;
	}

	public double getCurrOrientation() {
		return orientationHistory.getLast();
	}

	/**
	 * Correct the orientation value based on a noisy and delayed compass
	 * reading.
	 * 
	 * @param compassReading -
	 *            angle in degrees
	 */
	public void correctOrientation(double compassReading) {

		// degrees to radians
		final double reading = compassReading * (Math.PI / 180);

		if (orientationHistory.size() == Constants.COMPASS_DELAY + 1) {

			double deviation = AngleGeometry.angleDifference(orientationHistory
					.peek(), reading);

			// only update if the deviation is bigger than 2 times the square of
			// the
			// standard deviation squared
			if (Math.abs(deviation) > 2 * compassStdDev * compassStdDev) {
				orientationHistory.set(Constants.COMPASS_DELAY, AngleGeometry
						.normalizeAngle(orientationHistory.getLast()
								+ deviation));
			}
		}
	}

	public Point2D getHistoricPosition() {
		if (positionHistory.size() == Constants.BEACON_DELAY + 1)
			return positionHistory.peek();
		else
			return null;
	}

	public double getHistoricOrientation() {
		if (orientationHistory.size() == Constants.BEACON_DELAY + 1)
			return orientationHistory.peek();
		else
			return Double.NaN;
	}

	public double getLPowOut() {
		return lPowOut;
	}

	public double getRPowOut() {
		return rPowOut;
	}

	public Point2D getStartPos() {
		return this.startPos;
	}

}
